#include "MPU6050_SW.h"
#include <stdlib.h>
#include "Delay.h"
#include "bsp_led.h"

/* 灵敏度系数 */
#define Acc_LSBSens     (8192.0f)         // 加速度计灵敏度：满量程为+-4g时，1g == 8192
#define Gyro_LSBSens    (16.4f)           // 陀螺仪灵敏度：满量程为+-2000 deg/s时，16.4

/* 单位转换系数 */
#define G               (9.80665f)              // 重力加速度g，单位：m/s^2
#define Deg_To_Rad      (0.01745329f)           // pi/180
#define Acc_Gain        (0.001197100830)        // 乘以1.0f/Acc_LSBSens将单位转换为重力加速度g，再乘G转换为m/s2
#define Gyro_Gain_Deg_S (0.060975609756)        // 1.0f/Gyro_LSBSens, 单位转换为°/s
#define Gyro_Gain_Rad_S (0.001064224999)        // (Gyro_Gain_Deg_S) * (Deg_To_Rad)°/s转换成rad/s

/* 封装i2c读写函数 */
static void MPU_Write_Byte(uint8_t reg, uint8_t data);
static uint8_t MPU_Read_Byte(uint8_t reg);
static void MPU_Writ_Len(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf);
static void MPU_Read_Len(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf);

int16_t MpuOffset[6] = {0};		//MPU6050补偿数值


/**
 * @brief MPU6050写寄存器
 * 
 * @param reg 寄存器地址，范围：参考MPU6050手册的寄存器描述
 * @param data 要写入寄存器的数据，范围：0x00~0xFF
 */
static void MPU_Write_Byte(uint8_t reg, uint8_t data)
{
    MyI2C_Start();						//I2C起始
    MyI2C_SendByte(MPU_ADDR);	        //发送从机地址，读写位为0，表示即将写入
    MyI2C_ReceiveAck();					//接收应答
    MyI2C_SendByte(reg);			//发送寄存器地址
    MyI2C_ReceiveAck();					//接收应答
    MyI2C_SendByte(data);				//发送要写入寄存器的数据
    MyI2C_ReceiveAck();					//接收应答
    MyI2C_Stop();						//I2C终止
}

/**
 * @brief MPU6050读寄存器
 * 
 * @param reg 寄存器地址，范围：参考MPU6050手册的寄存器描述
 * @return uint8_t 读取寄存器的数据，范围：0x00~0xFF
 */
static uint8_t MPU_Read_Byte(uint8_t reg)
{
    uint8_t Data;
    
    MyI2C_Start();						//I2C起始
    MyI2C_SendByte(MPU_ADDR);	        //发送从机地址，读写位为0，表示即将写入
    MyI2C_ReceiveAck();					//接收应答
    MyI2C_SendByte(reg);			//发送寄存器地址
    MyI2C_ReceiveAck();					//接收应答
    
    MyI2C_Start();						//I2C重复起始
    MyI2C_SendByte(MPU_ADDR | 0x01);	//发送从机地址，读写位为1，表示即将读取
    MyI2C_ReceiveAck();					//接收应答
    Data = MyI2C_ReceiveByte();			//接收指定寄存器的数据
    MyI2C_SendAck(1);					//发送应答，给从机非应答，终止从机的数据输出
    MyI2C_Stop();						//I2C终止
    
    return Data;
}

static void MPU_Writ_Len(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf)
{
	int16_t i = 0;

	MyI2C_Start();
	MyI2C_SendByte(addr);
	MyI2C_ReceiveAck();	
	MyI2C_SendByte(reg);
	MyI2C_ReceiveAck();	
	for (i = 0; i < len; i++)
	{
		MyI2C_SendByte(buf[i]);
		MyI2C_ReceiveAck();
	}
	MyI2C_Stop();
}

static void MPU_Read_Len(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf)
{
    int16_t i = 0;

    MyI2C_Start();
    MyI2C_SendByte(addr);
    MyI2C_ReceiveAck();
    MyI2C_SendByte(reg);
    MyI2C_ReceiveAck();

    MyI2C_Start();
    MyI2C_SendByte(addr | 0x01);
    MyI2C_ReceiveAck();
    for (i = 0; i < len; i++)
    {
        buf[i] = MyI2C_ReceiveByte();
        if (i == len-1)
        {
            MyI2C_SendAck(0);       //最后一个字节不应答
        }
        else
        {
            MyI2C_SendAck(1);
        }
    }
    MyI2C_Stop();
}

/**
 * @brief MPU6050初始化
 * 
 * @return MPU6050_t* 
 */
MPU6050_t* MPU_Init()
{
    uint8_t ID = 0;

    /* 为MPU6050对象分配内存 */
    MPU6050_t *MPU = (MPU6050_t *)malloc(sizeof(MPU6050_t));
    if (MPU == NULL) {
        goto malloc_failed;
    }
    
    /* 初始化I2C */
    // MyI2C_Init();

    /* 配置MPU6050的参数 */
    MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0X80);	// 复位MPU6050
    Delay_ms(10);
    MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0X00);	// 唤醒MPU6050
    MPU_Set_Gyro_Fsr(3);					    // 陀螺仪传感器,±2000dps
    MPU_Set_Accel_Fsr(1);					    // 加速度传感器,±4g
    MPU_Set_Rate(50);						    // 设置采样率50Hz
    MPU_Write_Byte(MPU_INT_EN_REG, 0X00);	    // 关闭所有中断
    MPU_Write_Byte(MPU_USER_CTRL_REG, 0X00); 	// I2C主模式关闭
    MPU_Write_Byte(MPU_FIFO_EN_REG, 0X00);	    // 关闭FIFO
    MPU_Write_Byte(MPU_INTBP_CFG_REG, 0X80);    // INT引脚低电平有效

    /* 获取并检验ID号 */
    ID = MPU_GetID();
    MPU->devID = ID;
    if (ID == MPU_ID) {
        LED_R_ON;
        Delay_ms(500);
        LED_R_OFF;
        Delay_ms(500);
        LED_R_ON;
        Delay_ms(500);
        LED_R_OFF;
    }
    // printf("\nMPU6050:0x%2x\n", ID);

    /* 启动MPU6050 */
    MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0X01);	// 设置CLKSEL,PLL X轴为参考
    MPU_Write_Byte(MPU_PWR_MGMT2_REG, 0X00);	// 启动加速度与陀螺仪
    MPU_Set_Rate(50);						    // 设置采样率为50Hz
    return MPU;

malloc_failed:
    if (MPU != NULL) {
        free(MPU);
        MPU = NULL;
    }
    return NULL;
}

/**
 * @brief 复位MPU6050
 * 
 * @param MPU 
 * @return MPU_State 
 */
MPU_State MPU_DeInit(MPU6050_t *MPU)
{
    if (MPU != NULL) {
        free(MPU);
        MPU = NULL;
        return MPU_OK;
    }
    return MPU_ERROR;
}

/**
 * @brief 设置MPU6050陀螺仪传感器满量程范围,
 *
 * @param fsr 0 -> ±250dps;
 *            1 -> ±500dps;
 *            2 -> ±1000dps;
 *            3 -> ±2000dps
 * @return MPU_State MPU_OK, MPU_ERROR or MPU_TimeOut.
 */
void MPU_Set_Gyro_Fsr(uint8_t fsr)
{
    MPU_Write_Byte(MPU_GYRO_CFG_REG, fsr << 3);
}

/**
 * @brief 设置MPU6050加速度传感器满量程范围
 *
 * @param fsr 0 -> ±2g;
 *            1 -> ±4g;
 *            2 -> ±8g;
 *            3 -> ±16g
 * g = 9.8m/s2
 * @return MPU_State MPU_OK, MPU_ERROR or MPU_TimeOut.
 */
void MPU_Set_Accel_Fsr(uint8_t fsr)
{
    MPU_Write_Byte(MPU_ACCEL_CFG_REG, fsr << 3);
}

/**
 * @brief 设置MPU6050的数字低通滤波器
 *
 * @param lpf 数字低通滤波频率(Hz)
 * @return MPU_State MPU_OK, MPU_ERROR or MPU_TimeOut.
 */
void MPU_Set_LPF(uint16_t lpf)
{
    uint8_t data = 0;
    if (lpf >= 188)     data = 1;
    else if (lpf >= 98) data = 2;
    else if (lpf >= 42) data = 3;
    else if (lpf >= 20) data = 4;
    else if (lpf >= 10) data = 5;
    else                data = 6;
    MPU_Write_Byte(MPU_CFG_REG, data);
}

/**
 * @brief 设置MPU6050的采样率(假定Fs=1KHz)
 *
 * @param rate 4~1000(Hz)
 * @return MPU_State MPU_OK, MPU_ERROR or MPU_TimeOut.
 */
void MPU_Set_Rate(uint16_t rate)
{
    uint8_t data = 0;
    if (rate > 1000) rate = 1000;
    if (rate < 4)    rate = 4;
    data = 1000 / rate - 1;
    MPU_Write_Byte(MPU_SAMPLE_RATE_REG, data);
    MPU_Set_LPF(rate / 2);
}

/**
 * @brief 获取设备ID号
 *
 * @return uint8_t
 */
uint8_t MPU_GetID()
{
    return MPU_Read_Byte(MPU_DEVICE_ID_REG);
}

/**
 * @brief 获取温度值
 *
 * @return float 温度值（摄氏度）
 */
float MPU_GetTemperature(void)
{
    uint8_t buf[2] = {0};
    int16_t raw = 0;
    float temp = 0;

    buf[0] = MPU_Read_Byte(MPU_TEMP_OUTH_REG);
    buf[1] = MPU_Read_Byte(MPU_TEMP_OUTL_REG);
    raw = ((int16_t)buf[0] << 8) | buf[1];          // 合并高位和低位
    temp = ((float)raw / 340.0f + 36.53f ) * 100;   // 转换为摄氏度
    return temp / 100.0f;
}

/**
 * @brief 获取陀螺仪的角速度值(原始值)
 *
 * @param gx x轴角速度值
 * @param gy y轴角速度值
 * @param gz z轴角速度值
 * @return MPU_State MPU_OK, MPU_ERROR or MPU_TimeOut.
 */
MPU_State MPU_GetGyro_Raw(int16_t *gx, int16_t *gy, int16_t *gz)
{
    uint8_t buf[6] = {0};

    buf[0] = MPU_Read_Byte(MPU_GYRO_XOUTH_REG);
    buf[1] = MPU_Read_Byte(MPU_GYRO_XOUTL_REG);
    buf[2] = MPU_Read_Byte(MPU_GYRO_YOUTH_REG);
    buf[3] = MPU_Read_Byte(MPU_GYRO_YOUTL_REG);
    buf[4] = MPU_Read_Byte(MPU_GYRO_ZOUTH_REG);
    buf[5] = MPU_Read_Byte(MPU_GYRO_ZOUTL_REG);

    *gx = ((int16_t)buf[0] << 8) | buf[1];
    *gy = ((int16_t)buf[2] << 8) | buf[3];
    *gz = ((int16_t)buf[4] << 8) | buf[5];
}

/**
 * @brief 获取加速度计的加速度值(原始值)
 *
 * @param ax x轴的加速度
 * @param ay y轴的加速度
 * @param az z轴的加速度
 * @return MPU_State MPU_OK, MPU_ERROR or MPU_TimeOut.
 */
MPU_State MPU_GetAccel_Raw(int16_t *ax,int16_t *ay,int16_t *az)
{
    uint8_t buf[6] = {0};

    buf[0] = MPU_Read_Byte(MPU_ACCEL_XOUTH_REG);
    buf[1] = MPU_Read_Byte(MPU_ACCEL_XOUTL_REG);
    buf[2] = MPU_Read_Byte(MPU_ACCEL_YOUTH_REG);
    buf[3] = MPU_Read_Byte(MPU_ACCEL_YOUTL_REG);
    buf[4] = MPU_Read_Byte(MPU_ACCEL_ZOUTH_REG);
    buf[5] = MPU_Read_Byte(MPU_ACCEL_ZOUTL_REG);

    *ax = ((int16_t)buf[0] << 8) | buf[1];
    *ay = ((int16_t)buf[2] << 8) | buf[3];
    *az = ((int16_t)buf[4] << 8) | buf[5];
}

/**
 * @brief 获取陀螺仪的角速度值（单位°/s）
 *
 * @param gx x轴的角速度
 * @param gy y轴的角速度
 * @param gz z轴的角速度
 * @param unit 指定角速度的单位：
 *      - MPU_Deg_S -> °/s
 *      - MPU_Rad_S -> rad/s
 * @return MPU_State MPU_OK, MPU_ERROR or MPU_TimeOut.
 */
MPU_State MPU_GetGyro(float *gx, float *gy, float *gz, uint8_t unit)
{
    uint8_t buf[6] = {0};
    int16_t x = 0, y = 0 ,z = 0;

    /* 读取原始值 */
    buf[0] = MPU_Read_Byte(MPU_GYRO_XOUTH_REG);
    buf[1] = MPU_Read_Byte(MPU_GYRO_XOUTL_REG);
    buf[2] = MPU_Read_Byte(MPU_GYRO_YOUTH_REG);
    buf[3] = MPU_Read_Byte(MPU_GYRO_YOUTL_REG);
    buf[4] = MPU_Read_Byte(MPU_GYRO_ZOUTH_REG);
    buf[5] = MPU_Read_Byte(MPU_GYRO_ZOUTL_REG);

    x = ((int16_t)buf[0] << 8) | buf[1];
    y = ((int16_t)buf[2] << 8) | buf[3];
    z = ((int16_t)buf[4] << 8) | buf[5];

    if (unit == MPU_Deg_S) {
        /* 转换单位为：°/s */
        *gx = (float)x * Gyro_Gain_Deg_S;
        *gy = (float)y * Gyro_Gain_Deg_S;
        *gz = (float)z * Gyro_Gain_Deg_S;        
    } else if (unit == MPU_Rad_S) {
        /* 转换单位为：rad/s */
        *gx = (float)x * Gyro_Gain_Rad_S;
        *gy = (float)y * Gyro_Gain_Rad_S;
        *gz = (float)z * Gyro_Gain_Rad_S;        
    }

}

/**
 * @brief 获取加速度计的加速度值（单位m/s2）
 *
 * @param ax x轴的加速度
 * @param ay y轴的加速度
 * @param az z轴的加速度
 * @return MPU_State MPU_OK, MPU_ERROR or MPU_TimeOut.
 */
MPU_State MPU_GetAccel(float *ax, float *ay, float *az)
{
    uint8_t buf[6] = {0};
    int16_t x = 0, y = 0 ,z = 0;

    /* 读取原始值 */
    buf[0] = MPU_Read_Byte(MPU_ACCEL_XOUTH_REG);
    buf[1] = MPU_Read_Byte(MPU_ACCEL_XOUTL_REG);
    buf[2] = MPU_Read_Byte(MPU_ACCEL_YOUTH_REG);
    buf[3] = MPU_Read_Byte(MPU_ACCEL_YOUTL_REG);
    buf[4] = MPU_Read_Byte(MPU_ACCEL_ZOUTH_REG);
    buf[5] = MPU_Read_Byte(MPU_ACCEL_ZOUTL_REG);

    /* 合并高低位 */
    x = ((int16_t)buf[0] << 8) | buf[1];
    y = ((int16_t)buf[2] << 8) | buf[3];
    z = ((int16_t)buf[4] << 8) | buf[5];

    /* 转换单位为：m/s2 */
    *ax = (float)x * Acc_Gain;
    *ay = (float)y * Acc_Gain;
    *az = (float)z * Acc_Gain;
}

/**
 * @brief 获取MPU6050的数据，包括六轴加速度和温度
 * 
 * @param MPU 
 * @return MPU_State 
 */
MPU_State MPU_GetData(MPU6050_t *MPU)
{
    uint8_t buf[6] = {0};
    int16_t x = 0, y = 0 ,z = 0;
    int16_t raw = 0;
    float temp = 0;

    if (MPU->devID != MPU_ID) {
        return MPU_ERROR;
    }

    /* 读取陀螺仪原始值 */
    buf[0] = MPU_Read_Byte(MPU_GYRO_XOUTH_REG);
    buf[1] = MPU_Read_Byte(MPU_GYRO_XOUTL_REG);
    buf[2] = MPU_Read_Byte(MPU_GYRO_YOUTH_REG);
    buf[3] = MPU_Read_Byte(MPU_GYRO_YOUTL_REG);
    buf[4] = MPU_Read_Byte(MPU_GYRO_ZOUTH_REG);
    buf[5] = MPU_Read_Byte(MPU_GYRO_ZOUTL_REG);
    x = ((int16_t)buf[0] << 8) | buf[1];
    y = ((int16_t)buf[2] << 8) | buf[3];
    z = ((int16_t)buf[4] << 8) | buf[5];

    /* 转换单位为：rad/s */
    MPU->gx = (float)x * Gyro_Gain_Rad_S;
    MPU->gy = (float)y * Gyro_Gain_Rad_S;
    MPU->gz = (float)z * Gyro_Gain_Rad_S;        

    /* 读取加速度计原始值 */
    buf[0] = MPU_Read_Byte(MPU_ACCEL_XOUTH_REG);
    buf[1] = MPU_Read_Byte(MPU_ACCEL_XOUTL_REG);
    buf[2] = MPU_Read_Byte(MPU_ACCEL_YOUTH_REG);
    buf[3] = MPU_Read_Byte(MPU_ACCEL_YOUTL_REG);
    buf[4] = MPU_Read_Byte(MPU_ACCEL_ZOUTH_REG);
    buf[5] = MPU_Read_Byte(MPU_ACCEL_ZOUTL_REG);
    x = ((int16_t)buf[0] << 8) | buf[1];
    y = ((int16_t)buf[2] << 8) | buf[3];
    z = ((int16_t)buf[4] << 8) | buf[5];

    /* 转换单位为：m/s2 */
    MPU->ax = (float)x * Acc_Gain;
    MPU->ay = (float)y * Acc_Gain;
    MPU->az = (float)z * Acc_Gain;

    /* 获取温度 */
    buf[0] = MPU_Read_Byte(MPU_TEMP_OUTH_REG);
    buf[1] = MPU_Read_Byte(MPU_TEMP_OUTL_REG);
    raw = ((int16_t)buf[0] << 8) | buf[1];          // 合并高位和低位
    temp = ((float)raw / 340.0f + 36.53f ) * 100;   // 转换为摄氏度
    MPU->temp = temp / 100.0f;

    return MPU_OK;
}

